package Estados;

import teams.ucmTeam.RobotAPI;
import EDU.gatech.cc.is.util.Vec2;
import MaquinaEstados.Estado;
import MaquinaEstados.MaquinaEstados;

public class EstadoAtacanteDefendiendo extends Estado{
	
	public EstadoAtacanteDefendiendo(MaquinaEstados miMaquina) {
		super(miMaquina);
	}

	@Override
	protected void onEnd(RobotAPI robot) {
		robot.setDisplayString("");	
	}

	@Override
	protected void onInit(RobotAPI robot) {
		robot.setDisplayString("AtacanteDefendiendo");	
	}

	@Override
	protected void onTakeStep(RobotAPI robot) {
		boolean closestToBall = robot.closestToBall();
		boolean aligned = robot.alignedToBallandGoal();
		Vec2 ball= robot.getBall();	

		Vec2 goal = robot.getOpponentsGoal();
		Vec2 oponent = robot.getClosestOpponent();

		if (closestToBall){
			
			if (ball.r<0.1) {//Si estoy pegado miro a la porteria rival
				robot.setBehindBall(robot.getOpponentsGoal());
				robot.setSpeed(1.0);
				if ((goal.r<0.3) || (oponent.r<0.1)){ //se deshace del balon tirando
					robot.setSteerHeading(robot.getOpponentsGoal().t);
					if(robot.canKick()){
						robot.kick();
					}
				}
			}else { //miro al balon
				robot.setBehindBall(robot.getBall());
				robot.setSpeed(1.0);
			}

		}
		//si estoy cerca voy,
		else if (ball.r<0.6) {
			robot.setBehindBall(robot.getOpponentsGoal());
			if(robot.canKick()){
				robot.kick();
			}	

			
		} else{
			//si no, espero en el medio campo
			Vec2 destino = new Vec2(0, ball.y);
			Vec2 destEgo = robot.toEgocentricalCoordinates(destino);
			double t=destEgo.t;
			robot.setSteerHeading(t);
			robot.setSpeed(1.0);
			Vec2 dist = destEgo;
			dist.sub(robot.getPosition());
			if (dist.r<0.2){
				robot.setSpeed(0.0);
			}
		}
	}
}
